ROS2机器人编程简述humble |
您所在的位置：网站首页 › tf2 指令 › ROS2机器人编程简述humble |
|
书中程序适用于turtlebot、husky等多种机器人，配置相似都可以用的。 支持ROS2版本foxy、humble。 基础检测效果如下： 由于缺￥，所有设备都非常老旧，都是其他实验室淘汰或者拼凑出来的设备。机器人控制笔记本是2010年版本。 但是依然可以跑ROS1、ROS2。 book_ros2/br2_tf2_detector目录： . ├── CMakeLists.txt ├── include │ └── br2_tf2_detector │ ├── ObstacleDetectorImprovedNode.hpp │ ├── ObstacleDetectorNode.hpp │ └── ObstacleMonitorNode.hpp ├── launch │ ├── detector_basic.launch.py │ ├── detector_improved.launch.py │ ├── turtlebot_detector_basic.launch.py │ └── turtlebot_detector_improved.launch.py ├── package.xml └── src ├── br2_tf2_detector │ ├── ObstacleDetectorImprovedNode.cpp │ ├── ObstacleDetectorNode.cpp │ ├── ObstacleMonitorNode (copy).cpp │ └── ObstacleMonitorNode.cpp ├── detector_improved_main.cpp └── detector_main.cpp 5 directories, 15 files里面有两个部分basic和improved。 CMakelist（lib）： cmake_minimum_required(VERSION 3.5) project(br2_tf2_detector) set(CMAKE_CXX_STANDARD 17) # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) set(dependencies rclcpp tf2_ros geometry_msgs sensor_msgs visualization_msgs ) include_directories(include) add_library(${PROJECT_NAME} SHARED src/br2_tf2_detector/ObstacleDetectorNode.cpp src/br2_tf2_detector/ObstacleMonitorNode.cpp src/br2_tf2_detector/ObstacleDetectorImprovedNode.cpp ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) add_executable(detector src/detector_main.cpp) ament_target_dependencies(detector ${dependencies}) target_link_libraries(detector ${PROJECT_NAME}) add_executable(detector_improved src/detector_improved_main.cpp) ament_target_dependencies(detector_improved ${dependencies}) target_link_libraries(detector_improved ${PROJECT_NAME}) install(TARGETS ${PROJECT_NAME} detector detector_improved ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() ament_package()障碍物识别节点 // Copyright 2021 Intelligent Robotics Lab // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include "br2_tf2_detector/ObstacleDetectorNode.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/rclcpp.hpp" namespace br2_tf2_detector { using std::placeholders::_1; ObstacleDetectorNode::ObstacleDetectorNode() : Node("obstacle_detector") { scan_sub_ = create_subscription( "input_scan", rclcpp::SensorDataQoS(), std::bind(&ObstacleDetectorNode::scan_callback, this, _1)); tf_broadcaster_ = std::make_shared(*this); } void ObstacleDetectorNode::scan_callback(sensor_msgs::msg::LaserScan::UniquePtr msg) { double dist = msg->ranges[msg->ranges.size() / 2]; if (!std::isinf(dist)) { geometry_msgs::msg::TransformStamped detection_tf; detection_tf.header = msg->header; detection_tf.child_frame_id = "detected_obstacle"; detection_tf.transform.translation.x = msg->ranges[msg->ranges.size() / 2]; tf_broadcaster_->sendTransform(detection_tf); } } } // namespace br2_tf2_detector主要就是回调函数完成大部分功能。具体参考源代码即可。 障碍物监控节点： // Copyright 2021 Intelligent Robotics Lab // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include #include #include "br2_tf2_detector/ObstacleMonitorNode.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/rclcpp.hpp" namespace br2_tf2_detector { using namespace std::chrono_literals; ObstacleMonitorNode::ObstacleMonitorNode() : Node("obstacle_monitor"), tf_buffer_(), tf_listener_(tf_buffer_) { marker_pub_ = create_publisher("obstacle_marker", 1); timer_ = create_wall_timer( 500ms, std::bind(&ObstacleMonitorNode::control_cycle, this)); } void ObstacleMonitorNode::control_cycle() { geometry_msgs::msg::TransformStamped robot2obstacle; try { robot2obstacle = tf_buffer_.lookupTransform( "odom", "detected_obstacle", tf2::TimePointZero); } catch (tf2::TransformException & ex) { RCLCPP_WARN(get_logger(), "Obstacle transform not found: %s", ex.what()); return; } double x = robot2obstacle.transform.translation.x; double y = robot2obstacle.transform.translation.y; double z = robot2obstacle.transform.translation.z; double theta = atan2(y, x); RCLCPP_INFO( get_logger(), "Obstacle detected at (%lf m, %lf m, , %lf m) = %lf rads", x, y, z, theta); visualization_msgs::msg::Marker obstacle_arrow; obstacle_arrow.header.frame_id = "odom"; obstacle_arrow.header.stamp = now(); obstacle_arrow.type = visualization_msgs::msg::Marker::ARROW; obstacle_arrow.action = visualization_msgs::msg::Marker::ADD; obstacle_arrow.lifetime = rclcpp::Duration(1s); geometry_msgs::msg::Point start; start.x = 0.0; start.y = 0.0; start.z = 0.0; geometry_msgs::msg::Point end; end.x = x; end.y = y; end.z = z; obstacle_arrow.points = {start, end}; obstacle_arrow.color.r = 1.0; obstacle_arrow.color.g = 0.0; obstacle_arrow.color.b = 0.0; obstacle_arrow.color.a = 1.0; obstacle_arrow.scale.x = 0.02; obstacle_arrow.scale.y = 0.1; obstacle_arrow.scale.z = 0.1; marker_pub_->publish(obstacle_arrow); } } // namespace br2_tf2_detector代码和原始版本稍微有些不同。 重要部分： try { robot2obstacle = tf_buffer_.lookupTransform( "odom", "detected_obstacle", tf2::TimePointZero); } catch (tf2::TransformException & ex) { RCLCPP_WARN(get_logger(), "Obstacle transform not found: %s", ex.what()); return; } double x = robot2obstacle.transform.translation.x; double y = robot2obstacle.transform.translation.y; double z = robot2obstacle.transform.translation.z; double theta = atan2(y, x); RCLCPP_INFO( get_logger(), "Obstacle detected at (%lf m, %lf m, , %lf m) = %lf rads", x, y, z, theta);如果tf不能正常工作，会报错Obstacle transform not found： 例如odom没有 [detector-1] [WARN] [1676266943.177279939] [obstacle_monitor]: Obstacle transform not found: "odom" passed to lookupTransform argument target_frame does not exist.例如detected_obstacle没有 [detector-1] [WARN] [1676267019.166991316] [obstacle_monitor]: Obstacle transform not found: "detected_obstacle" passed to lookupTransform argument source_frame does not exist.需要思考并解决问题哦^_^ 如果都ok！那么"Obstacle detected at (%lf m, %lf m, , %lf m) = %lf rads"： 机器人在运动中所以角度和距离会不断变化。 此时如果查看： rqt 其中检测tf是由激光传感器测距给出的。 节点主题图： 这个代码主程序！ // Copyright 2021 Intelligent Robotics Lab // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include "br2_tf2_detector/ObstacleDetectorNode.hpp" #include "br2_tf2_detector/ObstacleMonitorNode.hpp" #include "rclcpp/rclcpp.hpp" int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto obstacle_detector = std::make_shared(); auto obstacle_monitor = std::make_shared(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(obstacle_detector->get_node_base_interface()); executor.add_node(obstacle_monitor->get_node_base_interface()); executor.spin(); rclcpp::shutdown(); return 0; }这里需要注意！ rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(obstacle_detector->get_node_base_interface()); executor.add_node(obstacle_monitor->get_node_base_interface());如果C++掌握一般推荐看一看： 蓝桥ROS机器人之现代C++学习笔记7.1 并行基础 多线程是如何实现的。 整个程序要跑起来： 终端1-gazebo仿真：ros2 launch turtlebot3_gazebo empty_world.launch.py ros2 launch turtlebot3_gazebo empty_world.launch.py [INFO] [launch]: All log files can be found below /home/zhangrelay/.ros/log/2023-02-13-13-43-10-244500-Aspire4741-10860 [INFO] [launch]: Default logging verbosity is set to INFO urdf_file_name : turtlebot3_burger.urdf [INFO] [gzserver-1]: process started with pid [10862] [INFO] [gzclient -2]: process started with pid [10864] [INFO] [ros2-3]: process started with pid [10868] [INFO] [robot_state_publisher-4]: process started with pid [10870] [robot_state_publisher-4] [WARN] [1676266991.467830827] [robot_state_publisher]: No robot_description parameter, but command-line argument available. Assuming argument is name of URDF file. This backwards compatibility fallback will be removed in the future. [robot_state_publisher-4] Parsing robot urdf xml string. [robot_state_publisher-4] Link base_link had 5 children [robot_state_publisher-4] Link caster_back_link had 0 children [robot_state_publisher-4] Link imu_link had 0 children [robot_state_publisher-4] Link base_scan had 0 children [robot_state_publisher-4] Link wheel_left_link had 0 children [robot_state_publisher-4] Link wheel_right_link had 0 children [robot_state_publisher-4] [INFO] [1676266991.472337172] [robot_state_publisher]: got segment base_footprint [robot_state_publisher-4] [INFO] [1676266991.472419811] [robot_state_publisher]: got segment base_link [robot_state_publisher-4] [INFO] [1676266991.472444636] [robot_state_publisher]: got segment base_scan [robot_state_publisher-4] [INFO] [1676266991.472465018] [robot_state_publisher]: got segment caster_back_link [robot_state_publisher-4] [INFO] [1676266991.472485972] [robot_state_publisher]: got segment imu_link [robot_state_publisher-4] [INFO] [1676266991.472505808] [robot_state_publisher]: got segment wheel_left_link [robot_state_publisher-4] [INFO] [1676266991.472525491] [robot_state_publisher]: got segment wheel_right_link [ros2-3] Set parameter successful [INFO] [ros2-3]: process has finished cleanly [pid 10868] [gzserver-1] [INFO] [1676266994.292818234] [turtlebot3_imu]: is unset, using default value of false to comply with REP 145 (world as orientation reference) [gzserver-1] [INFO] [1676266994.417396256] [turtlebot3_diff_drive]: Wheel pair 1 separation set to [0.160000m] [gzserver-1] [INFO] [1676266994.417528534] [turtlebot3_diff_drive]: Wheel pair 1 diameter set to [0.066000m] [gzserver-1] [INFO] [1676266994.420616206] [turtlebot3_diff_drive]: Subscribed to [/cmd_vel] [gzserver-1] [INFO] [1676266994.425994254] [turtlebot3_diff_drive]: Advertise odometry on [/odom] [gzserver-1] [INFO] [1676266994.428920116] [turtlebot3_diff_drive]: Publishing odom transforms between [odom] and [base_footprint] [gzserver-1] [INFO] [1676266994.460852885] [turtlebot3_joint_state]: Going to publish joint [wheel_left_joint] [gzserver-1] [INFO] [1676266994.461009035] [turtlebot3_joint_state]: Going to publish joint [wheel_right_joint]终端2-障碍物检测： ros2 launch br2_tf2_detector turtlebot_detector_basic.launch.py 终端3-rqt：rqt 终端4-rviz2：rviz2 windows端也可以获取信息。 补充： 四元数是方向的4元组表示，它比旋转矩阵更简洁。四元数对于分析涉及三维旋转的情况非常有效。四元数广泛应用于机器人、量子力学、计算机视觉和3D动画。 可以在维基百科上了解更多关于基础数学概念的信息。还可以观看一个可探索的视频系列，将3blue1brown制作的四元数可视化。 官方教程将指导完成调试典型tf2问题的步骤。它还将使用许多tf2调试工具，如tf2_echo、tf2_monitor和view_frames。 TF2完整教程提纲： tf2 许多tf2教程都适用于C++和Python。这些教程经过简化，可以完成C++曲目或Python曲目。如果想同时学习C++和Python，应该学习一次C++教程和一次Python教程。 目录 工作区设置 学习tf2 调试tf2 将传感器消息与tf2一起使用 工作区设置 如果尚未创建完成教程的工作空间，请遵循本教程。 学习tf2 tf2简介。 本教程将让了解tf2可以为您做什么。它在一个多机器人的例子中展示了一些tf2的力量，该例子使用了turtlesim。这还介绍了使用tf2_echo、view_frames和rviz。 编写静态广播（Python）（C++）。 本教程教如何向tf2广播静态坐标帧。 编写广播（Python）（C++）。 本教程教如何向tf2广播机器人的状态。 编写监听器（Python）（C++）。 本教程教如何使用tf2访问帧变换。 添加框架（Python）（C++）。 本教程教如何向tf2添加额外的固定帧。 使用时间（Python）（C++）。 本教程教使用lookup_transform函数中的超时来等待tf2树上的转换可用。 时间旅行（Python）（C++）。 本教程向介绍tf2的高级时间旅行功能。 调试tf2 四元数基本原理。 本教程介绍ROS 2中四元数的基本用法。 调试tf2问题。 本教程向介绍调试tf2相关问题的系统方法。 将传感器消息与tf2一起使用 对tf2_ros：：MessageFilter使用标记数据类型。 本教程教您如何使用tf2_ros：：MessageFilter处理标记的数据类型。 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |